# 研究机构：上海大学机自学院
# 研 究 生：王强
# 开发时间：2024/6/26 14:24
import math
import random
from datetime import datetime, timedelta
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import openpyxl
# 导入MN逻辑法模块
import New_MN_Logic
import GNN_MTT
import EKF_MTT
import copy
# 真实路径
class aijpda_based_mtt:
    def __init__(self):
        self.M = 4
        self.N = 3
        self.Life_count = 3                                                                    # 生命周期计数
        self.current_time = 0                                                                  # 记录每一时刻的时间
        self.scans = []
        self.Initial_P = np.diag(np.ones(4)) * 0.01  # 初始P设为0 ??? zeros(4, 4)
        self.Initial_S = np.diag(np.ones(4))
        self.last_ekf_P_S = []  # 滤波过程中需要用到的上一时刻状态
        self.associate_tracks = []  # 经过全局最近邻数据关联算法的到累计关联航迹库
        self.ekf_z_tracks = []  # 经过滤波后得到的目标航迹库
        self.x_ture = []
        self.y_ture = []
        self.ath_obs = []
        self.dia_obs = []
        self.ekf_mtt = EKF_MTT.EKF(px=0, vx=0, py=0, vy=0, w=0.01, T=1)
        self.ttm_data = []
        self.output_file = 'generated_lists.xlsx'
        self.wb = openpyxl.Workbook()
        self.ws = self.wb.active
        self.ws.title = "Generated Data"
        self.time_sign = '-'

    # 读取雷达目标检测数据数据

    def read_detection_data(self):
        path = './x_ture.xlsx'
        data_x = pd.read_excel(path, header=None, skiprows=[0])
        self.x_ture = data_x.values

        path = './y_ture.xlsx'
        data_y = pd.read_excel(path, header=None, skiprows=[0])
        self.y_ture = data_y.values

        path = './ath_obs.xlsx'
        data_ath_obs = pd.read_excel(path, header=None, skiprows=[0])
        self.ath_obs = data_ath_obs.values

        path = './dis_obs.xlsx'
        data_dis_obs = pd.read_excel(path, header=None, skiprows=[0])
        self.dis_obs = data_dis_obs.values

    def track_initiation(self, tracker):
        print(step)
        # tracker.track_id =
        # 调用航迹起始模块对航迹进行起始
        initiated_tracks = tracker.start_tracks(aijpdamtt.scans)  # 起始航迹
        # 航迹管理，将新起始的航迹存入到关联航迹库中，进行目标跟踪
        start_target_ids = [dict_entry[next(iter(dict_entry))] for dict_entry in
                            tracker.starting_tracks]  # 取出所有起始航迹所属目标的id值
        associate_ids = [dict_entry[next(iter(dict_entry))] for dict_entry in self.associate_tracks]  # 取起始航迹中已经进行关联的目标的id值
        # last_target_ids = [dict_entry[next(iter(dict_entry))] for dict_entry in last_ekf_P_S]
        # 遍历起始航迹中的每一条航迹，将新起始的航迹存入到关联航迹库中，以便于进行目标跟踪
        for t in range(len(tracker.starting_tracks)):
            # print(tracker.starting_tracks[t])
            # 将新起始的航迹存入到关联航迹库和航迹跟踪库，所起始的新航迹赋予初始的状态转移矩阵和协方差矩阵
            if start_target_ids[t] not in associate_ids:
                # 向关联航迹库存入新的起始航迹
                self.associate_tracks.append({
                    'id': start_target_ids[t],
                    "associate_points": tracker.starting_tracks[t]['points']
                })
                # 存入初始滤波算法所用的状态
                # print(tracker.starting_tracks[t]['points'])
                Z_obs_datas = []
                for i in range(len(tracker.starting_tracks[t]['points'])):
                    intial_z_data = tracker.starting_tracks[t]['points'][i]
                    # print('intial_z_data', intial_z_data)
                    x_obs = intial_z_data[0] * np.cos(np.array(intial_z_data[1]) / 180 * 3.14)
                    y_obs = intial_z_data[0] * np.sin(np.array(intial_z_data[1]) / 180 * 3.14)
                    if i == 0:
                        self.ekf_mtt = EKF_MTT.EKF(px=x_obs, vx=0, py=y_obs, vy=0, w=0.01, T=1)
                        self.last_ekf_P_S.append({
                            'id': start_target_ids[t],
                            'Life': self.Life_count,
                            'P': self.ekf_mtt.P,
                            'S': self.ekf_mtt.S,
                            'X': self.ekf_mtt.x,
                            'Next_Z': self.ekf_mtt.pre_nextz,
                            'min_distance': self.ekf_mtt.target_distance,
                            'min_time': 100
                        })
                    self.ekf_mtt.z = np.transpose([x_obs, y_obs])
                    self.ekf_mtt.P = self.last_ekf_P_S[t]['P']
                    self.ekf_mtt.x = self.last_ekf_P_S[t]['X']
                    self.ekf_mtt.KF_update()
                    # 存储中间状态（状态协方差方程P，S 和预测状态X）
                    self.last_ekf_P_S[t]['P'] = self.ekf_mtt.P
                    self.last_ekf_P_S[t]['X'] = self.ekf_mtt.x
                    self.last_ekf_P_S[t]['S'] = self.ekf_mtt.S
                    self.last_ekf_P_S[t]['Next_Z'] = self.ekf_mtt.pre_nextz
                    Z_obs_datas.append([np.linalg.norm([self.ekf_mtt.x[0], self.ekf_mtt.x[2]], ord=2),
                                        math.atan2(self.ekf_mtt.x[2], self.ekf_mtt.x[0]) / 3.14 * 180])
                    # 目标航迹库存入起始的航迹
                self.ekf_z_tracks.append({
                    'id': start_target_ids[t],
                    "ekf_points": Z_obs_datas
                })
                # print(start_target_ids)

        # ekf_z_tracks = copy.deepcopy(associate_tracks)

        # print(last_ekf_P_S)
        # print('ekf_z_tracks', ekf_z_tracks)
        # print(associate_tracks)
        self.scans.pop(0)  # 删除M次雷达扫描周期内的最早一批点迹
    # 航迹关联
    def track_correlation(self):
            for t in range(len(self.ekf_z_tracks)):
                # print(tracker.tracks[t]['id'])
                # print(tracker.tracks[t]['points'])
                # print(last_ekf_P_S[t]['Next_Z'])
                targets.append(self.last_ekf_P_S[t]['Next_Z'])
                S_datas.append(self.last_ekf_P_S[t]['S'])
                # print(S_datas)
            gnn_mtt = GNN_MTT.GNN(targets=targets, measurements=z_obs_datas)
            gnn_mtt.S = S_datas
            associate_tracks = gnn_mtt.global_nearest_neighbor(self.associate_tracks)       # tracker.tracks为实际观测的航迹，并未进行卡尔曼滤波更新得到准确的卡尔曼滤波
            # print(gnn_mtt.association_matrix)
            row_sums = gnn_mtt.association_matrix.sum(axis=1)                               # 关联矩阵 association_matrix 按行求和， 行为零对应的目标无量测关联，表示出现漏检，
            # 某一列为零，表示量测未关联
            # print(row_sums)
            # print(associated_track)
            return row_sums

    def Life_track_prediction(self, row_sums):
        # 模拟雷达出现漏检情况
        if random.random() <= 0.1:
            a = int(random.random()*3)
            row_sums[a] = 0
        for t in range(len(row_sums)):
            # len(associated_track[t]) == 1 表示当前时刻航迹无关联雷达量测，出现漏检，扩展生命周期管理方法，对航迹进行删除和管理
            # print(associated_track[t])
            if row_sums[t] == 0:
                if self.last_ekf_P_S[t]['Life'] == 0:
                    self.ekf_z_tracks.pop(t)                                        # 删除航迹目标跟踪数据库
                    print('批号为{}目标航迹信息已删除'.format(t))
                else:
                    self.last_ekf_P_S[t]['Life'] = self.last_ekf_P_S[t]['Life']-1
                    z_obs_data = self.last_ekf_P_S[t]['Next_Z']                     # 用下一时刻数据进行预测
                    self.track_prediction(t, z_obs_data)
                print('批号为{}目标航迹出现漏检测！！！'.format(t))
                print('当前目标生命周期计数为{}/4次(计数越小表明目标消失的概率越大)'.format(self.last_ekf_P_S[t]['Life']))
            # len(associated_track[t]) 不等于 1 表示当前时刻航迹成功关联雷达量测，使用EKF进行航机更新，对航迹进行维持
            else:
                self.last_ekf_P_S[t]['Life'] = np.min([self.Life_count + 1, self.last_ekf_P_S[t]['Life'] + 1])
                z_obs_data = self.associate_tracks[t]['associate_points'][-1]  # 用下一时刻数据进行预测
                self.track_prediction(t, z_obs_data)

                # print(ekf_z_tracks[t]['id'])
                # print(ekf_z_tracks[t]['points'][-2:])
        # 周期管理待定

    def track_prediction(self, t, z_obs_data):
        x_obs = z_obs_data[0] * np.cos(np.array(z_obs_data[1]) / 180 * 3.14)
        y_obs = z_obs_data[0] * np.sin(np.array(z_obs_data[1]) / 180 * 3.14)
        self.ekf_mtt.z = [x_obs, y_obs]
        self.ekf_mtt.P = self.last_ekf_P_S[t]['P']
        self.ekf_mtt.x = self.last_ekf_P_S[t]['X']
        self.ekf_mtt.KF_update()
        self.last_ekf_P_S[t]['Next_Z'] = self.ekf_mtt.pre_nextz
        # print(self.last_ekf_P_S[t]['Life'] )
        # 存储中间状态（状态协方差方程P，S 和预测状态X）
        self.last_ekf_P_S[t]['P'] = self.ekf_mtt.P
        self.last_ekf_P_S[t]['X'] = self.ekf_mtt.x
        self.last_ekf_P_S[t]['S'] = self.ekf_mtt.S
        # 存储最短距离
        if self.ekf_mtt.target_distance < self.last_ekf_P_S[t]['min_distance']:
            self.last_ekf_P_S[t]['min_distance'] = self.ekf_mtt.target_distance
            min_time = self.ekf_mtt.target_distance * 1852 / 20 / 60  # 假设无人艇自身速度为20m/s
            if min_time > self.last_ekf_P_S[t]['min_time']:
                self.time_sign = '-'
            else:
                self.time_sign = ''
            self.last_ekf_P_S[t]['min_time'] = min_time
        self.ttm_data = [t, self.ekf_mtt.target_distance, self.ekf_mtt.target_bearing, 'R', self.ekf_mtt.target_speed,
                         self.ekf_mtt.taeget_course, 'R',
                         self.last_ekf_P_S[t]['min_distance'], self.last_ekf_P_S[t]['min_time'], self.time_sign, 'Kn',
                         'USV: {}'.format(t + 1), '', 'Tracking', self.current_time, 'A', 'hh', '<CR><LF>']
        self.ws.append(self.ttm_data)
        self.ekf_z_tracks[t]["ekf_points"] += [[np.linalg.norm([self.ekf_mtt.x[0], self.ekf_mtt.x[2]], ord=2),
                                                math.atan2(self.ekf_mtt.x[2], self.ekf_mtt.x[0]) / 3.14 * 180]]

if __name__ == '__main__':
    # 读取当前时间并格式化为 "时分秒" 的字符串形式
    start_time = datetime.now()
    aijpdamtt = aijpda_based_mtt()
    aijpdamtt.read_detection_data()                     # 读取雷达目标检测每一时刻的距离和位置数据
    tracker = New_MN_Logic.TrackInitiation(threshold=100, M=aijpdamtt.M, N=aijpdamtt.N)     # 雷达航迹起始模块初始化
    for step in range(100):
        aijpdamtt.current_time = start_time + + timedelta(seconds=step)
        # step为时间长度
        # 雷达累计扫描M次后，累计连续扫描数据组数等于M次对航迹进行起始
        if len(aijpdamtt.scans) == aijpdamtt.M:
            aijpdamtt.track_initiation(tracker)
        if step < aijpdamtt.M:
            z_obs_datas = np.transpose([aijpdamtt.dis_obs[step], aijpdamtt.ath_obs[step]])
            # print(z_obs_datas)
            aijpdamtt.scans.append(z_obs_datas)
        else:
            targets = []                                                 # 获取下一时刻的雷达量测值
            S_datas = []                                                 # 协方差矩阵
            z_obs_datas = np.transpose([aijpdamtt.dis_obs[step], aijpdamtt.ath_obs[step]])
            # 全局最近邻数据关联算法，获得已关联的航迹
            # print(tracker.starting_tracks)
            row_sums = aijpdamtt.track_correlation()
            aijpdamtt.Life_track_prediction(row_sums)

# 输出数据
    aijpdamtt.wb.save(aijpdamtt.output_file)
    print(f"生成的列表已逐行存储到: {aijpdamtt.output_file}")
    print('ekf_z_tracks=', aijpdamtt.ekf_z_tracks)
    print(aijpdamtt.associate_tracks)
